#include "GLHeaders.hpp"
#include "Vector.hpp"
#include "Bezier.hpp"

// Constructor
Bezier::Bezier(void){
	v0 = Vector::ZERO;
	v1 = Vector::ZERO;
	v2 = Vector::ZERO;
	v3 = Vector::ZERO;
}

Bezier::Bezier(
		const Vector& vect0,
		const Vector& vect3){
	v0 = vect0;
	v1 = vect0;
	v2 = vect3;
	v3 = vect3;
}

Bezier::Bezier(
		const Vector& vect0,
		const Vector& vect1,
		const Vector& vect2,
		const Vector& vect3){
	v0 = vect0;
	v1 = vect1;
	v2 = vect2;
	v3 = vect3;
}



//Accessors
	Vector Bezier::GetVector(int nb) const{
		if(0 == nb)
			return Vector(v0);
		else if(1 == nb)
			return Vector(v1);
		else if(2 == nb)
			return Vector(v2);
		else if(3 == nb)
			return Vector(v3);
		else
			return Vector(Vector::ZERO);
	}

	// Warning time must belong to [0;1]
	Vector Bezier::GetPosition(GLfloat time) const{
		GLfloat t  = time;
		GLfloat t2 = t*time;
		GLfloat t3 = t2*time; 
		GLfloat f0 = 1 - 3*t + 3*t2 - t3;
		GLfloat f1 = 3*t - 6*t2 + 3*t3;
		GLfloat f2 = 3*t2 - 3*t3;
		GLfloat f3 = t3;

		return Vector(f0*v0 + f1*v1 + f2*v2 + f3*v3);
	}

	Vector Bezier::GetFirstDerivative(GLfloat time) const{
		GLfloat t  = time;
		GLfloat t2 = t*time;
		GLfloat f0 = -3 + 6*t - 3*t2;
		GLfloat f1 = 3 - 12*t + 9*t2;
		GLfloat f2 = 6*t - 9*t2;
		GLfloat f3 = 3*t2;

		return Vector(f0*v0 + f1*v1 + f2*v2 + f3*v3);
	}

	Vector Bezier::GetSecondDerivative(GLfloat time) const{
		GLfloat t  = time;
		GLfloat f0 = 6 - 6*t;
		GLfloat f1 = 12 + 18*t;
		GLfloat f2 = 6 - 18*t;
		GLfloat f3 = 6*t;

		return Vector(f0*v0 + f1*v1 + f2*v2 + f3*v3);
	}

//Mutators
	void Bezier::SetVector(int nb, Vector &vect){
		if(0 == nb)
			v0 = vect;
		else if(1 == nb)
			v1 = vect;
		else if (2 == nb)
			v2 = vect;
		else if (3 == nb)
			v3 = vect;
	}

//Special Bezier Curves
const Bezier Bezier::ZERO(
		Vector::ZERO,
		Vector::ZERO,
		Vector::ZERO,
		Vector::ZERO);

const Bezier Bezier::UNIT_X(
		Vector::ZERO,
		Vector::ZERO,
		Vector::UNIT_X,
		Vector::UNIT_X);

const Bezier Bezier::UNIT_Y(
		Vector::ZERO,
		Vector::ZERO,
		Vector::UNIT_Y,
		Vector::UNIT_Y);

const Bezier Bezier::UNIT_Z(
		Vector::ZERO,
		Vector::ZERO,
		Vector::UNIT_Z,
		Vector::UNIT_Z);

const Bezier Bezier::NEGATIVE_UNIT_X(
		Vector::ZERO,
		Vector::ZERO,
		Vector::NEGATIVE_UNIT_X,
		Vector::NEGATIVE_UNIT_X);

const Bezier Bezier::NEGATIVE_UNIT_Y(
		Vector::ZERO,
		Vector::ZERO,
		Vector::NEGATIVE_UNIT_Y,
		Vector::NEGATIVE_UNIT_Y);

const Bezier Bezier::NEGATIVE_UNIT_Z(
		Vector::ZERO,
		Vector::ZERO,
		Vector::NEGATIVE_UNIT_Z,
		Vector::NEGATIVE_UNIT_Z);

